#include "JCMergeMatcherI.h"
#include "Point.h"
#include <vector>
#include "AssociationSet.h"

using namespace std;

JCMergeMatcherI::JCMergeMatcherI()
{
	name = "JCMergeMatcherI";
	printf("new JCMergeMatcherI\n");
	nr_iter = 500;
	distance_threshold = 0.001f;
}

JCMergeMatcherI::~JCMergeMatcherI()
{
	printf("delete JCMergeMatcherI\n");
}

void JCMergeMatcherI::update(){
}

const bool debugg_JCMergeMatcherI = true;
Transformation * JCMergeMatcherI::getTransformation(RGBDFrame * src, RGBDFrame * dst)
{
	if(debugg_JCMergeMatcherI){printf("JCMergeMatcherI::getTransformation(%i,%i)\n",src->id,dst->id);}
	IplImage* img_combine;
	int width;
	int height;
	if(debugg_JCMergeMatcherI)
	{	
		width = src->width;
		height = src->height;
		
		img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_32F, 3);
		float * data = (float *)img_combine->imageData;
		int index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (width+i)) + 0] = float(dst->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 1] = float(dst->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 2] = float(dst->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (i)) + 0] = float(src->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (i)) + 1] = float(src->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (i)) + 2] = float(src->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		
		for(unsigned int i = 0; i < src->keypoints->valid_key_points.size(); i++){
			cvCircle(img_combine,cvPoint(src->keypoints->valid_key_points.at(i)->point->w, src->keypoints->valid_key_points.at(i)->point->h), 3,cvScalar(0, 0, 255, 0),1, 8, 0);
		}
		
		for(unsigned int i = 0; i < dst->keypoints->valid_key_points.size(); i++){
			cvCircle(img_combine,cvPoint(dst->keypoints->valid_key_points.at(i)->point->w + width, dst->keypoints->valid_key_points.at(i)->point->h), 3,cvScalar(0, 0, 255, 0),1, 8, 0);
		}
	}
	
	unsigned int nr_valid_src = src->keypoints->valid_key_points.size();
	unsigned int nr_valid_dst = dst->keypoints->valid_key_points.size();
	float limit = 0.07f;
	vector < AssociationSet * > all_asso;
	AssociationSet * full = new AssociationSet();
	for(unsigned int i = 0; i < nr_valid_src; i++){
		KeyPoint * src_kp = src->keypoints->valid_key_points.at(i);
		for(unsigned int j = 0; j < nr_valid_dst; j++){
			KeyPoint * dst_kp = dst->keypoints->valid_key_points.at(j);
			float dist = src_kp->descriptor->distance(dst_kp->descriptor);
			if(dist < limit){
				AssociationSet * a_set = new AssociationSet();
				a_set->add(new KeyPointAssociation(src_kp , dst_kp));
				all_asso.push_back(a_set);
				full->add(new KeyPointAssociation(src_kp , dst_kp));
				if(debugg_JCMergeMatcherI){
					printf("%i %i -> %f\n",i,j,dist);
					//cvLine(img_combine,cvPoint(dst_kp->point->w  + width ,dst_kp->point->h),cvPoint(src_kp->point->w,src_kp->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
				}
			}
		}
	}
	printf("nr_asso: %i\n",(int)full->selected_associations->size());
	float ** jc = full->jc();
	for(unsigned int i = 0; i < full->selected_associations->size(); i++){
		for(unsigned int j = 0; j < full->selected_associations->size(); j++){
			if(jc[i][j] >= 0){printf(" ");}
			printf("%.5f,",jc[i][j]);
		}
		printf("\n");
		printf("\n");
	}
	exit(0);
	int iter = 500;
	int total = all_asso.size();
	while( iter > 0 && total > 1)
	{
		printf("||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n");
		IplImage * img_combine_clone = cvCreateImage(cvSize(img_combine->width, img_combine->height), IPL_DEPTH_32F, 3);
		cvCopy( img_combine, img_combine_clone, NULL );
		
		int arand = rand()%total;
		int brand = rand()%total;
		AssociationSet * a = all_asso.at(arand);
		AssociationSet * b = all_asso.at(brand);
		
		for(unsigned int i = 0; i < a->selected_associations->size(); i++){
			Association * asso1 = a->selected_associations->at(i);
			KeyPointAssociation * a1 = (KeyPointAssociation *)asso1;
			Point * pa_src = a1->p1->point;
			Point * pa_dst = a1->p2->point;
			
			for(unsigned int j = 0; j < b->selected_associations->size(); j++){
				Association * asso2 = b->selected_associations->at(j);
				KeyPointAssociation * b1 = (KeyPointAssociation *)asso2;
				Point * pb_src = b1->p1->point;
				Point * pb_dst = b1->p2->point;				
				float diff = fabs(pa_src->distance(pb_src)-pa_dst->distance(pb_dst));
				//printf("+++diff %f\n",diff);
				if(diff < 0.10f){
					cvCircle(img_combine_clone,cvPoint(pa_src->w 		 , pa_src->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
					cvCircle(img_combine_clone,cvPoint(pa_dst->w + width , pa_dst->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
					cvLine  (img_combine_clone,cvPoint(pa_src->w 		 , pa_src->h),cvPoint(pa_dst->w + width , pa_dst->h),cvScalar(0, 255, 0, 0),1, 8, 0);
						
					cvCircle(img_combine_clone,cvPoint(pb_src->w 		 , pb_src->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
					cvCircle(img_combine_clone,cvPoint(pb_dst->w + width , pb_dst->h), 5,cvScalar(0, 255, 0, 0),2, 8, 0);
					cvLine  (img_combine_clone,cvPoint(pb_src->w 		 , pb_src->h),cvPoint(pb_dst->w + width , pb_dst->h),cvScalar(0, 255, 0, 0),1, 8, 0);
				}else{
					cvCircle(img_combine_clone,cvPoint(pa_src->w 		 , pa_src->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
					cvCircle(img_combine_clone,cvPoint(pa_dst->w + width , pa_dst->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
					cvLine  (img_combine_clone,cvPoint(pa_src->w 		 , pa_src->h),cvPoint(pa_dst->w + width , pa_dst->h),cvScalar(0, 0,255, 0),1, 8, 0);
						
					cvCircle(img_combine_clone,cvPoint(pb_src->w 		 , pb_src->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
					cvCircle(img_combine_clone,cvPoint(pb_dst->w + width , pb_dst->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);
					cvLine  (img_combine_clone,cvPoint(pb_src->w 		 , pb_src->h),cvPoint(pb_dst->w + width , pb_dst->h),cvScalar(0, 0, 255, 0),1, 8, 0);
				}
			}
		}
		//int mergeable = a->mergeable(b);

		
		int mergeable = a->mergeable(b);
		printf("mergeable = %i\n",mergeable);
		if(mergeable == 2){
			/*
			AssociationSet * merged = a->merge(b);
			delete a;
			delete b;
			all_asso.at(all_asso.size()-2) = merged;
			*/
			all_asso.at(arand)=all_asso.at(all_asso.size()-1);
			all_asso.at(brand)=all_asso.at(all_asso.size()-2);
			all_asso.pop_back();
			all_asso.pop_back();
			all_asso.push_back(a->merge(b));
			total = all_asso.size();
			
		}
		
		cvNamedWindow("combined image clone", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image clone", img_combine_clone);
		cvWaitKey(0);
		cvReleaseImage( &img_combine_clone );
		iter--;
		printf("iter: %i all_asso.size() %i\n",iter,(int)all_asso.size());
	}
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	
	if(debugg_JCMergeMatcherI)
	{
		cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image", img_combine);
		cvWaitKey(0);
		cvReleaseImage( &img_combine );
	}
	return transformation;
}
